import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.GridLayout;
import java.awt.Point;
import java.awt.geom.AffineTransform;
import java.awt.geom.Point2D;
import java.util.ArrayList;

import javax.swing.BorderFactory;
import javax.swing.JPanel;


public class RobotPanel extends JPanel{

	ArrayList<Robot> robots;
	ArrayList<Light> lights;
	
	public RobotPanel(){
		
		this.setLayout(new GridLayout(10,1));
		this.setBorder(BorderFactory.createLineBorder(Color.black));
		this.setSize(new Dimension(800,600));
		this.setPreferredSize(new Dimension(800,600));
		this.setVisible(true);
		
		robots = new ArrayList<Robot>();
		lights = new ArrayList<Light>();
	}

	public void paintComponent(Graphics g){
		super.paintComponent(g);
		Graphics2D g2d = (Graphics2D) g;
		
		g2d.setColor(Color.black);
		
		for(Robot rob: robots){
			int x = (int)rob.getPosX(); int y = (int)rob.getPosY();
			g2d.translate(5, 5);
			g2d.rotate(rob.getTheta(), x/2, y/2);
			
			AffineTransform T = g2d.getTransform();
			Point p = new Point((int)rob.getPosX(),(int)rob.getPosY());
			Point p2 = new Point();
			T.transform(p, p2);
			g2d.fillRect((int)rob.getPosX(), (int)rob.getPosY(), (int)rob.getWidth(), (int)rob.getHeight());
			
			g2d.rotate(-rob.getTheta(), x/2, y/2);
			g2d.translate(-5,-5);
			
			//update rectangles position
			rob.setPosX((int)p2.getX());
			rob.setPosY((int)p2.getY());
			rob.getSensor1().setPosX((int)(p2.getX()+ rob.getWidth()));
			rob.getSensor1().setPosY((int)(p2.getY()));
			rob.getSensor2().setPosX((int)(p2.getX()+ rob.getWidth()));
			rob.getSensor2().setPosY((int)(p2.getY()+ rob.getHeight()));
			
		}
		
		g2d.setColor(Color.yellow);
		for(Light light: lights){
			//int x = (int)rob.getPosX(); int y = (int)rob.getPosY();
			//g2d.rotate(rob.getTheta(), x/2, y/2);
			//g2d.translate(5, 5);
			g2d.fillOval(light.getPosX(), light.getPosY(), 10, 10);
			//g2d.rotate(-rob.getTheta(), x/2, y/2);
			//g2d.translate(-5,-5);
		}
	}
	
	public void addRobot(Robot rob)
	{
		robots.add(rob);
	}
	
	public void addLight(Light light)
	{
		lights.add(light);
	}
	
	public void clear()
	{
		lights.clear();
		robots.clear();
		repaint();
	}
	
	// find dist b/t rob and light
	// TODO:would probably be better to find distance from a sensor...
	public double distRobotLight(Robot rob, Light light)
	{
		return Math.sqrt(Math.pow(light.getPosX()-rob.getPosX(), 2) + Math.pow(light.getPosY()-rob.getPosY(), 2));
	}
	
	public void updateRobots()
	{
		System.out.println("here");
		if (lights.isEmpty() || robots.isEmpty())
			return;
		
		Light minDistLight = new Light(999,999,0);
		double dist = 9999999;
		double mindist = dist;
		for (Robot rob : robots) {
			// find the closest light for the robot
			for (Light light: lights) {
				dist = distRobotLight(rob, light);
				if (dist < mindist)
				{
					minDistLight = light;
					mindist = dist;
				}
			}
			
			// set speed and new position
			rob.setSpeed(minDistLight);
			rob.calcLocation();
			
			// reset stuff
			dist = 9999999;
			mindist = dist;
		}
		
		repaint();
	}
}
